/***************************************************************************
 创建者: 华磊
 开始时间: 2018.8.14
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2017.5.8 类的具体实现代码编写

 ***************************************************************************/
#include "forinstructionlogic.h"
#include "GeneralDefine.h"
#include "programengine.h"
#include "messagelog.h"

ForInstructionLogic::ForInstructionLogic(int robotIdIn,ProgramSet* programSetIn,
                                         RobotIo* ioControllerIn,ProgramEngine* programEngineIn)
{
  programSet=programSetIn;
  robotId=robotIdIn;
  ioController=ioControllerIn;
  programEngine=programEngineIn;
}

E_PROGRAM_LOOP_RESULT ForInstructionLogic::startFor(int programNum,ForInstruction forInstruct)
{
    programEngine->updateProgramSetRegister();

    E_PROGRAM_LOOP_RESULT endFlag = E_PROGRAM_LOOP_RESULT_NORMAL;

    int regValue,tarValue;
    ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
//    qDebug() << "****====="<<programSet->r[forInstruct.registerNum -1]<<"==registerNum==="<<forInstruct.registerNum<<endl;
    switch(forInstruct.actionType)
    {
    case TO:
    {
        //            qDebug()<<"&&&&&&&"<<__LINE__<<endl;
        forInstruct.forFlag = true;
        break;
    }
    case DOWNTO:
    {
        ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
        forInstruct.forFlag = false;
        break;
    }
    }
    ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
    switch(forInstruct.targetType)
    {
    ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
    case TARGET_CONST:
    {
        ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
        tarValue = forInstruct.targetValue;
        qDebug()<<"&&&&&&&"<<__LINE__<<"tarValue****"<<tarValue<<
                  "forInstruct.targetValue*****"<<forInstruct.targetValue<<endl;
        break;
    }
    case TARGET_R:
    {
        //            //qDebug()<<"&&&&&&&"<<__LINE__<<"tarValue****"<<tarValue<<
        //                      "forInstruct.targetValue*****"<<forInstruct.targetValue<<endl;
        if(D_ROBOT_REG_SIZE < forInstruct.targetValue ||1 > forInstruct.targetValue )
        {

            addMsg( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3017,  robotId);
            programEngine->cmdError(programNum);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        else
        {
            tarValue = programSet->regRList.at(forInstruct.targetValue - 1);
            qDebug()<<"&&&&&&&"<<__LINE__<<"tarValue****"<<tarValue<<"===forInstruct.targetValue*****"<<forInstruct.targetValue<<endl;
        }
        break;
    }
    case TARGET_AR:
    {
        break;
    }
    }
    ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
    if((*programSet)[programNum][programEngine->getRunningLineFloat()].forInstruct.firstFlag == false)
    {
        if(false == (*programSet)[programNum][programEngine->getRunningLineFloat()].forInstruct.forCompleteFlag)
        {
            if(D_ROBOT_REG_SIZE < forInstruct.registerNum || 1 > forInstruct.registerNum)
            {

                addMsg( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3017,  robotId);
                programEngine->cmdError(programNum);
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                regValue = programSet->regRList.at(forInstruct.registerNum - 1);
            }

            if(forInstruct.forFlag)
            {
                if(regValue <= tarValue)
                {
                    ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
                    programSet->regRList[forInstruct.registerNum - 1] += 1;
                }
            }
            else
            {
                if(regValue >= tarValue)
                {
                    ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
                    programSet->regRList[forInstruct.registerNum -1] -= 1;
                }
            }
            ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
            regValue = programSet->regRList.at(forInstruct.registerNum - 1);
            qDebug()<<"&&&&&&&"<<__LINE__<<"tarValue****"<<tarValue<<"****registerNum**"<<forInstruct.registerNum<<
                      "==regValue==="<<regValue<<endl;
        }
        else
        {
            switch(forInstruct.initialType)
            {
            case INITIAL_CONST:
            {
                regValue = forInstruct.initialValue;
                break;
            }
            case FOR_R:
            {
                if(D_ROBOT_REG_SIZE < forInstruct.initialValue || 1 > forInstruct.initialValue)
                {

                    addMsg( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3017,  robotId);
                    programEngine->cmdError(programNum);
                    return E_PROGRAM_LOOP_RESULT_ERROR;
                }
                else
                {
                    regValue = programSet->regRList.at(forInstruct.initialValue -1);
                }
                ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
                break;
            }
            case FOR_AR:
            {
                break;
            }
            }
            qDebug()<<"&&&&&&&"<<__LINE__<<"forInstruct.registerNum********"<<
                      forInstruct.registerNum<<"regValue------"<<regValue<<"**tarValue**"<<tarValue<<endl;

            if(D_ROBOT_REG_SIZE < forInstruct.registerNum || 1 > forInstruct.registerNum)
            {

                addMsg( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3017,  robotId);
                programEngine->cmdError(programNum);
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                programSet->regRList[forInstruct.registerNum -1] = regValue;
            }
            ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
        }
    }
    else
    {
        switch(forInstruct.initialType)
        {
        case INITIAL_CONST:
        {
            regValue = forInstruct.initialValue;
            break;
        }
        case FOR_R:
        {
            if(D_ROBOT_REG_SIZE < forInstruct.initialValue || 1 > forInstruct.initialValue)
            {

                addMsg( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3017,  robotId);
                programEngine->cmdError(programNum);
                return E_PROGRAM_LOOP_RESULT_ERROR;
            }
            else
            {
                regValue = programSet->regRList.at(forInstruct.initialValue -1);
            }
            ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
            break;
        }
        case FOR_AR:
        {
            break;
        }
        }
        qDebug()<<"&&&&&&&"<<__LINE__<<"forInstruct.registerNum********"<<
                  forInstruct.registerNum<<"regValue------"<<regValue<<"**tarValue**"<<tarValue<<endl;

        if(D_ROBOT_REG_SIZE < forInstruct.registerNum || 1 >  forInstruct.registerNum)
        {

            addMsg( ENUM_MSG_ERROR,  "ProgramEngine", "ProgramEngine",  3017,  robotId);
            programEngine->cmdError(programNum);
            return E_PROGRAM_LOOP_RESULT_ERROR;
        }
        else
        {
            programSet->regRList[forInstruct.registerNum -1] = regValue;
        }
        ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
    }


    ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
    for(int j = programEngine->getRunningLineFloat();j < (*programSet)[programNum].count();j++)
    {
        ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;

        if((*programSet)[programNum][programEngine->getRunningLineFloat()].forInstruct.firstFlag == false)
        {
            break;
        }

        if((*programSet)[programNum][j].type == FOR)
        {
            ++(*programSet)[programNum].forCount;
        }
        if((*programSet)[programNum][j].type == ENDFOR)
        {
            --(*programSet)[programNum].forCount;
        }

        if((*programSet)[programNum].forCount == 0)
        {

            (*programSet)[programNum].pairEndFor.first = j;
            (*programSet)[programNum].pairEndFor.second = tarValue;

            (*programSet)[programNum].mapPairFor.first = programEngine->getRunningLineFloat();
            (*programSet)[programNum].mapPairFor.second =  (*programSet)[programNum].pairEndFor;

            //            qDebug()<<"&&&&&&&"<<__LINE__<<
            //                       "j-----"<<j<<"tarValue-----"<<tarValue<<
            //                       "PC_count-----"<<pCount<<endl;
            break;
        }

    }

    //qDebug()<<"&&&&&&&"<<__LINE__<<endl;
    if((*programSet)[programNum][programEngine->getRunningLineFloat()].forInstruct.firstFlag == true)
    {
        //mapListfor.insert(programNum,mapPairFor);
        //        qDebug()<<"&&&&&&&"<<__LINE__<<endl;
        (*programSet)[programNum].mapListFor.insert(programNum,(*programSet)[programNum].mapPairFor);
    }


    //QList<mapPairFor> values = mapListfor.find(programNum);
    //qDebug()<<"&&&&&&&"<<__LINE__<<"****r******"<<programtmpRRegisteret->tmpR[forInstruct.registerNum]<<"****registerNum***"<<forInstruct.registerNum<<endl;
    QList<MapPairFor> values = (*programSet)[programNum].mapListFor.values(programNum);

    (*programSet)[programNum][programEngine->getRunningLineFloat()].forInstruct.firstFlag = false;

    int tmpCount = programEngine->getRunningLineFloat();
    if(forInstruct.forFlag)
    {
        //        qDebug()<<"&&&&&&&"<<__LINE__<<endl;
        if(regValue > tarValue)
        {
            //            qDebug()<<"&&&&&&&"<<__LINE__<<"**size**"<<values.size()<<endl;
            for(int k = 0; k < values.size();k++)
            {
                //                qDebug()<<"&&&&&&&"<<__LINE__<<"***k=**"<<k<<"**pc**"<<pCount<<"**first**"<<values.at(k).first<<endl;
                if(values.at(k).first == tmpCount)
                {
                    //                    qDebug()<<"&&&&&&&"<<__LINE__<<"**se**first**"<<values.at(k).second.first<<endl;

                    (*programSet)[programNum][programEngine->getRunningLineFloat()].forInstruct.forCompleteFlag = true;
                    (*programSet)[programNum].runningLineInt = values.at(k).second.first  ;
//                    pCount = (*programSet)[programNum].pc+0.02;

                    break;
                }
            }
        }
        else
        {
            (*programSet)[programNum][programEngine->getRunningLineFloat()].forInstruct.forCompleteFlag = false;
        }
        programEngine->calculateNextLineToRun(programNum);
    }
    else
    {
        tmpCount = programEngine->getRunningLineFloat();
        ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
        if(regValue < tarValue)
        {
            ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
            for(int k = 0; k < values.size();k++)
            {
                ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
                if(values.at(k).first == tmpCount)
                {
                    ////qDebug()<<"&&&&&&&"<<__LINE__<<endl;
                    (*programSet)[programNum][programEngine->getRunningLineFloat()].forInstruct.forCompleteFlag = true;
                    (*programSet)[programNum].runningLineInt = values.at(k).second.first ;
//                    pCount = (*programSet)[programNum].pc+0.02;
                    break;
                }
            }
        }
        else
        {
            (*programSet)[programNum][programEngine->getRunningLineFloat()].forInstruct.forCompleteFlag = false;
        }

        programEngine->calculateNextLineToRun(programNum);

    }



    endFlag = programEngine->isTheLastInstruction(programNum);

    return endFlag;
    //qDebug()<<"startFor*************(*programSet)[programNum].pc**************"<<(*programSet)[programNum].pc<<endl;
}

E_PROGRAM_LOOP_RESULT ForInstructionLogic::startEndfor(int programNum)
{

    E_PROGRAM_LOOP_RESULT endFlag = E_PROGRAM_LOOP_RESULT_NORMAL;

    if( programEngine->getRunModel()<ENUM_CONTROL_AUTO && programEngine->getMoveType()==ENUM_MOVE_ORDER_STEP)
    {
        programEngine->set_mamual_run_flag(false);
    }

    int tmpCount = programEngine->getRunningLineFloat();
    QList<MapPairFor> values = (*programSet)[programNum].mapListFor.values(programNum);
    for(int i = 0; i < values.size();i++)
    {
        if(values.at(i).second.first == tmpCount)
        {
            (*programSet)[programNum].runningLineInt = values.at(i).first;
//            pCount = (*programSet)[programNum].pc+0.02;

            programEngine->setPcFlagValue(0.0);
            break;
        }
        else
        {
            if(i == values.size() - 1)
            {
                //消息预留
                //报错处理，ｆｏｒ、ｅｎｄｆｏｒ指令不匹配
                //                processprogramEngine->getMoveType()(programNum);

                addMsg(ENUM_MSG_REMIND,"ProgramEngine","ProgramEngine",3018,robotId);
                programEngine->cmdError(programNum);
                qDebug()  <<"***3432 报错处理，ｆｏｒ、ｅｎｄｆｏｒ指令不匹配***" <<"pc====="<<programEngine->getRunningLineFloat()<<endl;
                return E_PROGRAM_LOOP_RESULT_ERROR;
//                break;
            }
        }
    }

    endFlag = programEngine->isTheLastInstruction(programNum);

    return endFlag;
    //    qDebug()<<"startendFor*************(*programSet)[programNum].pc**************"<<(*programSet)[programNum].pc<<endl;

}

int ForInstructionLogic::reset()
{
    if( programSet->size()>0 )
    {
        for(int i =0; i < programSet->size();i++)
        {
            for(int j = 0; j < (*programSet)[i].count();j++)
            {
                (*programSet)[i][j].forInstruct.forCompleteFlag = true;
            }
        }
    }
    return 1;
}


void ForInstructionLogic::addMsg(int messageLevel, string componentName, string messageType,
                           int messageCode, int robotId,
                           int parameter1, int parameter2, int parameter3, int parameter4,QString message)
{
    Message tmpMsg;
    tmpMsg.MessageLevel = messageLevel;
    tmpMsg.componentClassName = componentName;
    tmpMsg.messageType = messageType;
    tmpMsg.robotId = robotId;
    tmpMsg.messageCode = messageCode;
   // tmpMsg.time = t1.tv_sec;


    QString infomationStr;

    switch(messageCode)
    {

    case 3017:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",寄存器指令错误，寄存器的范围超过限定的最大范围，请修改");
    }
        break;
    case 3018:
    {
        infomationStr =  QObject::tr("机器人:")  + QString::number(robotId);
        infomationStr = infomationStr + QObject::tr(",for、endfor指令不匹配，请查看并予以修改");
    }
        break;



    default:
    {

    }
    }


    tmpMsg.MessageInformation = infomationStr.toStdString();

    // qDebug()<<"53444444444444444444444"<< tmpMsg.messageCode;
    MessageLog::getInstance()->addMessage(tmpMsg);
}
